#include "main.h"
#include "fuzzy_pid.h"


/**
  ******************************************************************************
  * @file    fuzzy_pid.c
  * @author  DKX
  * @version 支持电流模糊自适应PID和位置模糊自适应PID                   
  * @date    2023年03月30日  
  * @brief   模糊PID的相关控制参数在Fuzzy_PID_Control函数中,结构体所对应的是Fuzzy_PID_Control_S
	           下面我来介绍一下控制参数的意义
						 
						 Input_val_max  输入控制信号的最大值,用来计算闭环的反馈,因此如果改变了输入信号的范围
						                要去手动去更改(模拟输入),上位机自动去输入
														
						 Input_val_min  与上面类同
						 
						 E_S            误差E对于反馈的区间范围的缩放,调试过程中看其处于的范围值(在论域的上下
						                极限内),配合模糊规则设计会去改变实际的KP、KI、KD调整值
						 EC_S           与上面类同,只不过是误差变化率EC
						 
						 KP_DYMIC       KP调整值的最大值,意思是每次最大的KP变动范围为[-KP_DYMIC,KP_DYMIC]
						 KI_DYMIC       与上面类同
						 KD_DYMIC       与上面类同
						 
						 KP_Max         限制KP的最大值
						 KI_Max         限制KI的最大值
						 KD_Max         限制KD的最大值
						 
						 Num_Index      模糊子集的大小,数值要 0<Num_Index<N   越大计算越费时,但精度高,反之就反
						 
						 Switch_on      模糊PID开关
						 
						 Type           用来标志,防止传入不一样的类型
  ******************************************************************************
*/



//模糊自适应PID,定义E为(PID当次误差)，EC(这次误差与上次误差的误差变化率)

//论域的值个数
#define N 7

//语言变量
#define NB -3   //负大
#define NM -2   //负中
#define NS -1   //负小
#define ZO 0    //零
#define PS 1    //正小
#define PM 2    //正中
#define PB 3    //正大


/**********************************************************************************************/
/*模糊规则设计
	X轴为E   Y轴为EC
					NB NM NS ZO PS PM PB
					Y0 Y1 Y2 Y3 Y4 Y5 Y6
	NB	X0
	NM	X1 
	NS	X2
	ZO	X3
	PS	X4
	PM	X5
	PB	X6
*/


/*
模糊控制表
***Kp模糊规则设计***
设计标准，因为是跟随信号，如果E的误差过大说明KP的值太小了，因此需要更改KP的值，因此四个角都为正大或者正小
*/
int kp_matrix[7][7]={	
				{PB,PM,PS,ZO,ZO,PS,PS},
				{PM,PM,PS,ZO,ZO,ZO,ZO},
				{PS,PS,ZO,NS,ZO,ZO,ZO}, 
				{ZO,ZO,NS,NS,NS,ZO,ZO},
				{ZO,ZO,ZO,NS,ZO,PS,PS},
				{PS,PS,PS,ZO,PS,PM,PM},
				{PS,PS,PS,ZO,ZO,PM,PB}    };
		
/*
***Ki模糊规则设计***
*/
int ki_matrix[7][7]={	
				{PB,PM,PS,ZO,PS,PS,PS},
				{PM,PM,PS,ZO,PS,PS,PS},
				{PS,PS,PS,NS,ZO,ZO,ZO},
				{PS,PS,ZO,NS,ZO,PS,PS},
				{ZO,ZO,NS,NS,NS,ZO,ZO},
				{PS,PS,PS,ZO,PS,PM,PM},
				{PS,PS,PS,ZO,ZO,PM,PB}    };		
				
/*
***Kd模糊规则设计***		
*/
int kd_matrix[7][7]={	
				{PS,PS,ZO,ZO,ZO,PS,PS},
				{PS,PS,ZO,ZO,ZO,PS,PS},
				{ZO,ZO,ZO,NS,NS,ZO,ZO},
				{ZO,ZO,NS,NS,NS,NS,ZO},
				{ZO,ZO,ZO,NS,ZO,ZO,ZO},
				{PS,PS,PS,ZO,PS,PS,PS},
				{PS,PS,ZO,ZO,PS,PS,PS}    };

/**********************************************************************************************/


//模糊PID参数的控制	
void Fuzzy_PID_Control(Fuzzy_PID_S *Fuzzy_pid,Fuzzy_PID_Control_S* Fuzzy_pid_control,int Type)
{
		/*这里因为随时会去改变输入信号的区间[Fuzzy_pid_control->Input_val_min,Fuzzy_pid_control->Input_val_max]
	    因此需要随时改变我们闭环所反馈的范围,同时改变我们的E_K,EC_K,
	    KP_DYMIC,KI_DYMIC,KD_DYMIC是改变我们在更改kp、ki、kd值的变动范围
	  */
	
		if(Type==Current_Fuzzy_PID)   //设置电流闭环范围
		{
				Fuzzy_pid->Gather_T=NULL_Structure.current+Fuzzy_pid_control->Input_val_max*NULL_Structure.current_ratio;   //电流反馈区间最高值
				Fuzzy_pid->Gather_B=NULL_Structure.current+Fuzzy_pid_control->Input_val_min*NULL_Structure.current_ratio;   //电流反馈区间最低值
		}
	  else if(Type==Position_Fuzzy_PID)//设置位置闭环范围
		{
				Fuzzy_pid->Gather_T=NULL_Structure.position+Fuzzy_pid_control->Input_val_max*NULL_Structure.position_ratio; //位置反馈区间最高值
				Fuzzy_pid->Gather_B=NULL_Structure.position+Fuzzy_pid_control->Input_val_min*NULL_Structure.position_ratio; //位置反馈区间最低值
		}
    
		Fuzzy_pid->E_K=(float)Fuzzy_pid->Fuzzy_Pid_D_T/(Fuzzy_pid_control->E_S*(Fuzzy_pid->Gather_T-Fuzzy_pid->Gather_B));    //量化因子
		//
		Fuzzy_pid->EC_K=(float)Fuzzy_pid->Fuzzy_Pid_D_T/(Fuzzy_pid_control->EC_S*(Fuzzy_pid->Gather_T-Fuzzy_pid->Gather_B));  //量化因子
	
		Fuzzy_pid->KP_K=Fuzzy_pid_control->KP_DYMIC/Fuzzy_pid->Fuzzy_Pid_D_T;     //反模糊化kp参数
		Fuzzy_pid->KI_K=Fuzzy_pid_control->KI_DYMIC/Fuzzy_pid->Fuzzy_Pid_D_T;     //反模糊化ki参数
		//Fuzzy_pid->KD_K=Fuzzy_pid_control->KD_DYMIC/Fuzzy_pid->Fuzzy_Pid_D_T;     //反模糊化kd参数
}	


//模糊PID初始化
void Fuzzy_PID_init(Fuzzy_PID_S* Fuzzy_pid,Fuzzy_PID_Control_S* Fuzzy_pid_control,int Type)
{
	  
	  //**************************控制参数初始化****************************
	  Fuzzy_pid_control->Switch_on=0;   //默认关闭
	
	  Fuzzy_pid_control->E_S=0.1f;       //E误差缩放成全量程的0.1
	  Fuzzy_pid_control->EC_S=0.05f;     //EC误差缩放成全量程的0.05
	  
	  //根据实际的要去Fuzzy_PID_Control去更改
		Fuzzy_pid_control->Input_val_max=1500;   //初始化定义控制输入最大值为1500
	  Fuzzy_pid_control->Input_val_min=1500;   //初始化定义控制输入最小值为1500
	
		Fuzzy_pid_control->KP_DYMIC=2.0f;
		Fuzzy_pid_control->KI_DYMIC=0.05f;
		Fuzzy_pid_control->KD_DYMIC=0.0001f;
	
		Fuzzy_pid_control->KP_Max=50.0f;
	  Fuzzy_pid_control->KI_Max=1.0f;
	  Fuzzy_pid_control->KD_Max=0.001f;
	
		Fuzzy_pid_control->Num_Index=3;   //模糊子集初始化设置成3个
	
	  Fuzzy_pid_control->Type=Type;     //设置模糊PID控制的类型
	
		
		//**************************模糊PID相关初始化**************************
	  
		//论域[Fuzzy_Pid_D_B,Fuzzy_Pid_D_T]  TOP,BOTTOM
		Fuzzy_pid->Fuzzy_Pid_D_T=3 ;     //论域最大值
		Fuzzy_pid->Fuzzy_Pid_D_B=-3;     //论域最小值
	
		//采集的最大最小值,[Gather_B,Gather_T]
	  if(Type==Current_Fuzzy_PID)  //设置电流闭环范围
		{
				Fuzzy_pid->Gather_T=NULL_Structure.current+Fuzzy_pid_control->Input_val_max*NULL_Structure.current_ratio;   //电流反馈区间最高值
				Fuzzy_pid->Gather_B=NULL_Structure.current+Fuzzy_pid_control->Input_val_min*NULL_Structure.current_ratio;   //电流反馈区间最低值
		}
	  else
		{
				Fuzzy_pid->Gather_T=NULL_Structure.position+Fuzzy_pid_control->Input_val_max*NULL_Structure.position_ratio; //位置反馈区间最高值
				Fuzzy_pid->Gather_B=NULL_Structure.position+Fuzzy_pid_control->Input_val_min*NULL_Structure.position_ratio; //位置反馈区间最低值
		}
		//E与EC的论域系数
		/*
		计算方式，例如采集到的信号为1000到2000,则E的最大值变化为-1000到1000,对应
		[Fuzzy_Pid_D_B,Fuzzy_Pid_D_T],除以正值最大的差值即可的到相应的系数
		以后计算E对应的论域即可通过 E/E_K得到
		*/
		Fuzzy_pid->E_K=(float)Fuzzy_pid->Fuzzy_Pid_D_T/(Fuzzy_pid_control->E_S*(Fuzzy_pid->Gather_T-Fuzzy_pid->Gather_B));    //量化因子
		//
		Fuzzy_pid->EC_K=(float)Fuzzy_pid->Fuzzy_Pid_D_T/(Fuzzy_pid_control->EC_S*(Fuzzy_pid->Gather_T-Fuzzy_pid->Gather_B));  //量化因子
		
		Fuzzy_pid->E_D_P=0.0f;
		Fuzzy_pid->EC_D_P=0.0f;
		
		Fuzzy_pid->KP=0.0f;
		Fuzzy_pid->KI=0.0f;
		Fuzzy_pid->KD=0.0f;
		 
		Fuzzy_pid->KP_K=Fuzzy_pid_control->KP_DYMIC/Fuzzy_pid->Fuzzy_Pid_D_T;     //反模糊化kp参数
		Fuzzy_pid->KI_K=Fuzzy_pid_control->KI_DYMIC/Fuzzy_pid->Fuzzy_Pid_D_T;     //反模糊化ki参数
		Fuzzy_pid->KD_K=Fuzzy_pid_control->KD_DYMIC/Fuzzy_pid->Fuzzy_Pid_D_T;     //反模糊化kd参数

		Fuzzy_pid->LastError=0.0f;     //上次误差
		Fuzzy_pid->PrevError=0.0f;     //上上次误差
		
		Fuzzy_pid->K1=0.0f;
		Fuzzy_pid->K2=0.0f;
		Fuzzy_pid->K3=0.0f;
		
		Fuzzy_pid->PWM_ZERO=PWM_MID_VAL;
		Fuzzy_pid->PWM_out=PWM_MID_VAL;           //采用互补输出因此这里是MID_VAL
		
		Fuzzy_pid->Pwm_Out_Max=PWM_FULL_VAL-50;   //限制PWM输出的最大值
		Fuzzy_pid->Pwm_Out_Min=PWM_MIN_VAL +50;    //限制PWM输出的最小值
		
		Fuzzy_pid->det_PWM=0.0f;     //pwm的变化值初始化
		
		Fuzzy_pid->Type=Type;    //设置模糊PID的类型
		
		Fuzzy_pid->Init_Flag=1;    //初始化完成标记
		
}


//三角隶属度函数
float FTri(float x,float a,float b,float c)
{
		 float u;
		 if(x>=a&&x<=b)
			 u=(x-a)/(b-a);
		 else if(x>b&&x<=c)
			 u=(c-x)/(c-b);
		 else
			 u=0.0;
		 return u;
}


//梯形左求隶属度函数
float FTraL(float x,float a,float b)//FuzzyTrapezoidLeft
{
		if(x<=a)  
			return 1;
		else if((a<x)&&(x<=b))
			return (b-x)/(b-a);
		else if(x>b)
			return 0;
		else
			return 0;
}

//梯形右求隶属度函数
float FTraR(float x,float a,float b)//FuzzyTrapezoidRight
{
		if(x<=a)
			return 0;
		if((a<x)&&(x<b))
			return (x-a)/(b-a);
		if(x>=b)
			return 1;
		else
			return 1;
}



//可视化参数，以下参数用来看模糊PID中的变化情况
float delta_kp_v=0.0f;  //delta kp
float delta_ki_v=0.0f;  //delta ki



//模糊PID实现
void Fuzzy_PID_Realize(Fuzzy_PID_S *Fuzzy_pid,Fuzzy_PID_Control_S* Fuzzy_pid_control,float Target,float Feedback)
{
		 int f_p;
		 //用于计算三角隶属度的参数,梯形中只用l和m两个
		 int tri_l=Fuzzy_pid->Fuzzy_Pid_D_B; //-3
		 int tri_m=tri_l+1;                  //-2
		 int tri_r=tri_l+2;                  //-1
		 
		 //U值 E  EC  (输出的U值)
		 float U_E[N]={0},U_EC[N]={0};
		 
		 //为简化计算一个输入只激活三个模糊子集
		 const int Index_num=Fuzzy_pid_control->Num_Index;
		 int E_Index[Index_num],EC_Index[Index_num];
		 int e_j=0,ec_j=0;
		
		 //kp ki kd 的变化值
		 float det_kp,det_ki,det_kd;    //delta
		 
		 Fuzzy_pid->E=Target-Feedback;             //当前误差
		 Fuzzy_pid->EC=Fuzzy_pid->E-Fuzzy_pid->LastError; //误差变化率=当前误差-上次误差
		 
		 //得到论域中的值
		 Fuzzy_pid->E_D_P=Fuzzy_pid->E*Fuzzy_pid->E_K;
		 Fuzzy_pid->EC_D_P=Fuzzy_pid->EC*Fuzzy_pid->EC_K;

			//计算隶属度
		 for(f_p=0;f_p<N;f_p++)
		 {
				if(f_p==0)
				{
						U_E[0]=FTraL(Fuzzy_pid->E_D_P,Fuzzy_pid->Fuzzy_Pid_D_B,Fuzzy_pid->Fuzzy_Pid_D_B+1);    //-3 -2
						U_EC[0]=FTraL(Fuzzy_pid->EC_D_P,Fuzzy_pid->Fuzzy_Pid_D_B,Fuzzy_pid->Fuzzy_Pid_D_B+1);  //-3 -2
				}
				else if(f_p==N-1)
				{
						U_E[N-1]=FTraR(Fuzzy_pid->E_D_P,Fuzzy_pid->Fuzzy_Pid_D_T-1,Fuzzy_pid->Fuzzy_Pid_D_T);    //2 3
						U_EC[N-1]=FTraR(Fuzzy_pid->EC_D_P,Fuzzy_pid->Fuzzy_Pid_D_T-1,Fuzzy_pid->Fuzzy_Pid_D_T);  //2 3
				}
				else
				{
						U_E[f_p]=FTri(Fuzzy_pid->E_D_P,tri_l,tri_m,tri_r);
						U_EC[f_p]=FTri(Fuzzy_pid->EC_D_P,tri_l,tri_m,tri_r);
						tri_l++;
						tri_m++;
						tri_r++;
				
				}
				//加入模糊子集当中
				if(U_E[f_p]!=0&&e_j<Index_num)
				{
						E_Index[e_j++]=f_p;
				}
				if(U_EC[f_p]!=0&&ec_j<Index_num)
				{
						EC_Index[ec_j++]=f_p;
				}
		 }
				 
		 int q=0,w=0;
				 
		 //输出量等与各个隶属度函数乘模糊规则设计表上的参数
		 //例如我kp的在E,EC的规则表上在-3,-3处隶属度为1，因为隶属度的总和最大为1，因此查表得
		 //在kp -3，-3处为PB  PB为3   PB*1(PB×隶属度)为输出量  在乘相应的KP的量化因子就得到deltkp
		 float kp_num=0.0f,ki_num=0.0f,kd_num=0.0f;   
				 
		 //因为我们为了简化运算，取了Index_num个子集，隶属度的总和不一定为1，因此在计算的时候要除以一个实际的隶属度和
		 float kp_den=0.0f,ki_den=0.0f,kd_den=0.0f;
		 
		 
		 //计算u值，实际输出值
		 for(q=0;q<e_j;++q)
		 {
				for(w=0;w<ec_j;++w)
				{
						float tmp=U_E[E_Index[q]]*U_EC[EC_Index[w]];
						kp_num+=tmp*kp_matrix[E_Index[q]][EC_Index[w]];
						ki_num+=tmp*ki_matrix[E_Index[q]][EC_Index[w]];
						//kd_num+=tmp*kd_matrix[E_Index[q]][EC_Index[w]];
					
						kp_den+=tmp;
						ki_den+=tmp;
						//kd_den+=tmp;
				}
		 }
		
		 det_kp=(kp_num/kp_den)*Fuzzy_pid->KP_K;
		 det_ki=(kp_num/kp_den)*Fuzzy_pid->KI_K;
		 //det_kd=(kp_num/kp_den)*Fuzzy_pid->KD_K;
		 
		 
		 //可视化参数********调试的时候看的,没用
		 delta_kp_v=det_kp;
		 delta_ki_v=det_ki;
		 //可视化参数********
		 
		 
		 //计算新的KP、KI、KD
		 Fuzzy_pid->KP+=det_kp;
		 Fuzzy_pid->KI+=det_ki;
		 
		 Fuzzy_pid->KD=0;     //取消微分作用
		 //Fuzzy_pid->KD=det_kd;
		 
		 //限幅，防止KP，KI低于0
		 if(Fuzzy_pid->KP>=Fuzzy_pid_control->KP_Max) Fuzzy_pid->KP=Fuzzy_pid_control->KP_Max;
		 if(Fuzzy_pid->KP<=0) Fuzzy_pid->KP=0;
		 if(Fuzzy_pid->KI>=Fuzzy_pid_control->KI_Max) Fuzzy_pid->KI=Fuzzy_pid_control->KI_Max;
		 if(Fuzzy_pid->KI<=0) Fuzzy_pid->KI=0;
		 
		 //计算K1,K2,K3
		 Fuzzy_pid->K1=Fuzzy_pid->KP+Fuzzy_pid->KI+Fuzzy_pid->KD;
		 Fuzzy_pid->K2=-(Fuzzy_pid->KP+2*Fuzzy_pid->KD);
		 Fuzzy_pid->K3=Fuzzy_pid->KD;
		 
		 //计算det_PWM
		 Fuzzy_pid->det_PWM+=Fuzzy_pid->det_PWM+(Fuzzy_pid->K1*Fuzzy_pid->E)+(Fuzzy_pid->K2*Fuzzy_pid->LastError)+(Fuzzy_pid->K3*Fuzzy_pid->PrevError);
		 
		 //调整pwm输出值
		 //Fuzzy_pid->PWM_out=Fuzzy_pid->PWM_ZERO+Fuzzy_pid->det_PWM;
		 Fuzzy_pid->PWM_out=Fuzzy_pid->PWM_ZERO+ Fuzzy_pid->det_PWM;
		 
		 if(Fuzzy_pid->PWM_out>=Fuzzy_pid->Pwm_Out_Max) Fuzzy_pid->PWM_out=Fuzzy_pid->Pwm_Out_Max;
		 if(Fuzzy_pid->PWM_out<=Fuzzy_pid->Pwm_Out_Min) Fuzzy_pid->PWM_out=Fuzzy_pid->Pwm_Out_Min;
		
		 Fuzzy_pid->PrevError=Fuzzy_pid->LastError;  //将上次误差赋值给上上次误差
		 Fuzzy_pid->LastError=Fuzzy_pid->E; //将这次误差赋值给上一次误差

}

//模糊PID
void Fuzzy_PID(void)
{
	  //这里只有初始化完成，且模糊PID打开，且传入的两个参数的类型相同才会进入
		if(CURRENT_FUZZY_PID.Init_Flag==1&&CURRENT_FUZZY_PID_CONTROL.Switch_on==1&&POSITION_FUZZY_PID_CONTROL.Switch_on==0&&CURRENT_FUZZY_PID.Type==CURRENT_FUZZY_PID_CONTROL.Type)//单模糊自适应电流PID
		{
			  Fuzzy_PID_Control(&CURRENT_FUZZY_PID , &CURRENT_FUZZY_PID_CONTROL , Current_Fuzzy_PID);//电流控制
				
			  Fuzzy_PID_Realize(&CURRENT_FUZZY_PID , &CURRENT_FUZZY_PID_CONTROL , ADC1ConvertedVoltage5*NULL_Structure.current_ratio + NULL_Structure.current + MY_DITHER.chanzhen_val_current, (float)ADC2ConvertedVoltage1);//电流闭环
			
			  TIM_SetCompare1(TIM1,CURRENT_FUZZY_PID.PWM_out);	//修改占空比
		}
		else if(POSITION_FUZZY_PID.Init_Flag==1&&POSITION_FUZZY_PID_CONTROL.Switch_on==1&&CURRENT_FUZZY_PID_CONTROL.Switch_on==0&&POSITION_FUZZY_PID.Type==POSITION_FUZZY_PID_CONTROL.Type)//单模糊自适应位置PID
		{
				Fuzzy_PID_Control(&POSITION_FUZZY_PID , &POSITION_FUZZY_PID_CONTROL , Position_Fuzzy_PID); //位置控制
				
				Fuzzy_PID_Realize(&POSITION_FUZZY_PID , &POSITION_FUZZY_PID_CONTROL , ADC1ConvertedVoltage5*NULL_Structure.position_ratio + NULL_Structure.position + MY_DITHER.chanzhen_val_postion, (float)ADC1ConvertedVoltage2);//位置闭环
			
				TIM_SetCompare1(TIM1,POSITION_FUZZY_PID.PWM_out);	//修改占空比
		}
		else if(CURRENT_FUZZY_PID.Init_Flag==1&&POSITION_FUZZY_PID.Init_Flag==1&&CURRENT_FUZZY_PID_CONTROL.Switch_on==1&&POSITION_FUZZY_PID_CONTROL.Switch_on==1&&CURRENT_FUZZY_PID.Type==CURRENT_FUZZY_PID_CONTROL.Type) //模糊自适应双闭环
		{
				Fuzzy_PID_Control(&CURRENT_FUZZY_PID , &CURRENT_FUZZY_PID_CONTROL , Current_Fuzzy_PID);   //电流控制
			  
			  Fuzzy_PID_Control(&POSITION_FUZZY_PID , &POSITION_FUZZY_PID_CONTROL , Position_Fuzzy_PID); //位置控制
			  
			  Fuzzy_PID_Realize(&POSITION_FUZZY_PID , &POSITION_FUZZY_PID_CONTROL , ADC1ConvertedVoltage5*NULL_Structure.position_ratio + NULL_Structure.position + MY_DITHER.chanzhen_val_postion , (float)ADC1ConvertedVoltage2); //位置闭环
			  
			  Fuzzy_PID_Realize(&CURRENT_FUZZY_PID , &CURRENT_FUZZY_PID_CONTROL , POSITION_FUZZY_PID.det_PWM , (float)ADC2ConvertedVoltage1); //电流闭环
			  
			  TIM_SetCompare1(TIM1,CURRENT_FUZZY_PID.PWM_out);	//修改占空比
		}
}

